/*
 * img_processing.c
 *
 *  Created on: 2021年3月24日
 *      Author: Jiarun
 */

#include "img_processing.h"
#include <stdlib.h>


/*****图像所用行范围*****/
//uint8 MIX_distance =    20,
//      diff_valid_line = 52;


unsigned char Image_Use[USEH][USEW];
unsigned char Bin_Image[USEH][USEW];

/*************************************************************************
 *  函数名称：short GetOSTU (unsigned char tmImage[USEH][USEW])
 *  功能说明：大津法求阈值大小
 *  参数说明：tmImage ： 图像数据
 *  函数返回：无
 *  修改时间：2011年10月28日
 *  备    注：  GetOSTU(Image_Use);//大津法阈值
Ostu方法又名最大类间差方法，通过统计整个图像的直方图特性来实现全局阈值T的自动选取，其算法步骤为：
1) 先计算图像的直方图，即将图像所有的像素点按照0~255共256个bin，统计落在每个bin的像素点数量
2) 归一化直方图，也即将每个bin中像素点数量除以总的像素点
3) i表示分类的阈值，也即一个灰度级，从0开始迭代 1
4) 通过归一化的直方图，统计0~i 灰度级的像素(假设像素值在此范围的像素叫做前景像素) 所占整幅图像
        的比例w0，        并统计前景像素的平均灰度u0；统计i~255灰度级的像素(假设像素值在此范围的像素叫做背
        景像素)  * 所占整幅图像的比例w1，并统计背景像素的平均灰度u1；
5) 计算前景像素和背景像素的方差 g = w0*w1*(u0-u1) (u0-u1)
6) i++；转到4)，直到i为256时结束迭代
7) 将最大g相应的i值作为图像的全局阈值
缺陷:OSTU算法在处理光照不均匀的图像的时候，效果会明显不好，因为利用的是全局像素信息。
*************************************************************************/
short GetOSTU (unsigned char tmImage[USEH][USEW])
{
    signed short i, j;
    unsigned long Amount = 0;
    unsigned long PixelBack = 0;
    unsigned long PixelshortegralBack = 0;
    unsigned long Pixelshortegral = 0;
    signed long PixelshortegralFore = 0;
    signed long PixelFore = 0;
    float OmegaBack, OmegaFore, MicroBack, MicroFore, SigmaB, Sigma; // 类间方差;
    signed short MinValue, MaxValue;
    signed short Threshold = 0;
    unsigned char HistoGram[256];              //

    for (j = 0; j < 256; j++)
        HistoGram[j] = 0; //初始化灰度直方图

    for (j = 0; j < USEH; j++)
    {
        for (i = 0; i < USEW; i++)
        {
            HistoGram[tmImage[j][i]]++; //统计灰度级中每个像素在整幅图像中的个数
        }
    }

    for (MinValue = 0; MinValue < 256 && HistoGram[MinValue] == 0; MinValue++);        //获取最小灰度的值
    for (MaxValue = 255; MaxValue > MinValue && HistoGram[MinValue] == 0; MaxValue--); //获取最大灰度的值

    if (MaxValue == MinValue)
        return MaxValue;         // 图像中只有一个颜色
    if (MinValue + 1 == MaxValue)
        return MinValue;        // 图像中只有二个颜色

    for (j = MinValue; j <= MaxValue; j++)
        Amount += HistoGram[j];        //  像素总数

    Pixelshortegral = 0;
    for (j = MinValue; j <= MaxValue; j++)
    {
        Pixelshortegral += HistoGram[j] * j;        //灰度值总数
    }
    SigmaB = -1;
    for (j = MinValue; j < MaxValue; j++)
    {
        PixelBack = PixelBack + HistoGram[j];     //前景像素点数
        PixelFore = Amount - PixelBack;           //背景像素点数
        OmegaBack = (float) PixelBack / Amount;   //前景像素百分比
        OmegaFore = (float) PixelFore / Amount;   //背景像素百分比
        PixelshortegralBack += HistoGram[j] * j;  //前景灰度值
        PixelshortegralFore = Pixelshortegral - PixelshortegralBack;  //背景灰度值
        MicroBack = (float) PixelshortegralBack / PixelBack;   //前景灰度百分比
        MicroFore = (float) PixelshortegralFore / PixelFore;   //背景灰度百分比
        Sigma = OmegaBack * OmegaFore * (MicroBack - MicroFore) * (MicroBack - MicroFore);   //计算类间方差
        if (Sigma > SigmaB)                    //遍历最大的类间方差g //找出最大类间方差以及对应的阈值
        {
            SigmaB = Sigma;
            Threshold = j;
        }
    }
    return Threshold;                        //返回最佳阈值;
}

/*!
 * @brief    基于soble边沿检测算子的一种边沿检测
 *
 * @param    imageIn    输入数组
 *           imageOut   输出数组      保存的二值化后的边沿信息
 *           Threshold  阈值
 *
 * @return
 *
 * @note
 *
 * @example
 *
 * @date     2020/5/15
 */
void lq_sobel (unsigned char imageIn[USEH][USEW], unsigned char imageOut[USEH][USEW], unsigned char Threshold)
{
    /** 卷积核大小 */
    short KERNEL_SIZE = 3;
    short xStart = KERNEL_SIZE / 2;
    short xEnd = USEW - KERNEL_SIZE / 2;
    short yStart = KERNEL_SIZE / 2;
    short yEnd = USEH - KERNEL_SIZE / 2;
    short i, j, k;
    short temp[4];
    for (i = yStart; i < yEnd; i++)
    {
        for (j = xStart; j < xEnd; j++)
        {
            /* 计算不同方向梯度幅值  */
            temp[0] = -(short) imageIn[i - 1][j - 1] + (short) imageIn[i - 1][j + 1]     //{{-1, 0, 1},
            - (short) imageIn[i][j - 1] + (short) imageIn[i][j + 1]        // {-1, 0, 1},
            - (short) imageIn[i + 1][j - 1] + (short) imageIn[i + 1][j + 1];    // {-1, 0, 1}};

            temp[1] = -(short) imageIn[i - 1][j - 1] + (short) imageIn[i + 1][j - 1]     //{{-1, -1, -1},
            - (short) imageIn[i - 1][j] + (short) imageIn[i + 1][j]       // { 0,  0,  0},
            - (short) imageIn[i - 1][j + 1] + (short) imageIn[i + 1][j + 1];    // { 1,  1,  1}};

            temp[2] = -(short) imageIn[i - 1][j] + (short) imageIn[i][j - 1]       //  0, -1, -1
            - (short) imageIn[i][j + 1] + (short) imageIn[i + 1][j]       //  1,  0, -1
            - (short) imageIn[i - 1][j + 1] + (short) imageIn[i + 1][j - 1];    //  1,  1,  0

            temp[3] = -(short) imageIn[i - 1][j] + (short) imageIn[i][j + 1]       // -1, -1,  0
            - (short) imageIn[i][j - 1] + (short) imageIn[i + 1][j]       // -1,  0,  1
            - (short) imageIn[i - 1][j - 1] + (short) imageIn[i + 1][j + 1];    //  0,  1,  1

            temp[0] = abs(temp[0]);
            temp[1] = abs(temp[1]);
            temp[2] = abs(temp[2]);
            temp[3] = abs(temp[3]);

            /* 找出梯度幅值最大值  */
            for (k = 1; k < 4; k++)
            {
                if (temp[0] < temp[k])
                {
                    temp[0] = temp[k];
                }
            }

            if (temp[0] > Threshold)
            {
                imageOut[i][j] = 1;
            }
            else
            {
                imageOut[i][j] = 0;
            }
        }
    }
}

/*!
 * @brief    基于soble边沿检测算子的一种自动阈值边沿检测
 *
 * @param    imageIn    输入数组
 *           imageOut   输出数组      保存的二值化后的边沿信息
 *
 * @return
 *
 * @note
 *
 * @example
 *
 * @date     2020/5/15
 */
void lq_sobelAutoThreshold (unsigned char imageIn[USEH][USEW], unsigned char imageOut[USEH][USEW])
{
    /** 卷积核大小 */
    short KERNEL_SIZE = 7;
    short xStart = KERNEL_SIZE / 2;
    short xEnd = USEW - KERNEL_SIZE / 2;
    short yStart = KERNEL_SIZE / 2;
    short yEnd = USEH - KERNEL_SIZE / 2;
    short i, j, k;
    short temp[4];
    for (i = yStart; i < yEnd; i++)
    {
        for (j = xStart; j < xEnd; j++)
        {
            /* 计算不同方向梯度幅值  */
            temp[0] = -(short) imageIn[i - 1][j - 1] + (short) imageIn[i - 1][j + 1]     //{{-1, 0, 1},
            - (short) imageIn[i][j - 1] + (short) imageIn[i][j + 1]       // {-1, 0, 1},
            - (short) imageIn[i + 1][j - 1] + (short) imageIn[i + 1][j + 1];    // {-1, 0, 1}};

            temp[1] = -(short) imageIn[i - 1][j - 1] + (short) imageIn[i + 1][j - 1]     //{{-1, -1, -1},
            - (short) imageIn[i - 1][j] + (short) imageIn[i + 1][j]       // { 0,  0,  0},
            - (short) imageIn[i - 1][j + 1] + (short) imageIn[i + 1][j + 1];    // { 1,  1,  1}};

            temp[2] = -(short) imageIn[i - 1][j] + (short) imageIn[i][j - 1]       //  0, -1, -1
            - (short) imageIn[i][j + 1] + (short) imageIn[i + 1][j]       //  1,  0, -1
            - (short) imageIn[i - 1][j + 1] + (short) imageIn[i + 1][j - 1];    //  1,  1,  0

            temp[3] = -(short) imageIn[i - 1][j] + (short) imageIn[i][j + 1]       // -1, -1,  0
            - (short) imageIn[i][j - 1] + (short) imageIn[i + 1][j]       // -1,  0,  1
            - (short) imageIn[i - 1][j - 1] + (short) imageIn[i + 1][j + 1];    //  0,  1,  1

            temp[0] = abs(temp[0]);
            temp[1] = abs(temp[1]);
            temp[2] = abs(temp[2]);
            temp[3] = abs(temp[3]);

            /* 找出梯度幅值最大值  */
            for (k = 1; k < 4; k++)
            {
                if (temp[0] < temp[k])
                {
                    temp[0] = temp[k];
                }
            }

            /* 使用像素点邻域内像素点之和的一定比例    作为阈值  */
            temp[3] = (short) imageIn[i - 1][j - 1] + (short) imageIn[i - 1][j] + (short) imageIn[i - 1][j + 1]
                    + (short) imageIn[i][j - 1] + (short) imageIn[i][j] + (short) imageIn[i][j + 1]
                    + (short) imageIn[i + 1][j - 1] + (short) imageIn[i + 1][j] + (short) imageIn[i + 1][j + 1];

            if (temp[0] > temp[3] / 12.0f)
            {
                imageOut[i][j] = 1;
            }
            else
            {
                imageOut[i][j] = 0;
            }

        }
    }
}

/*************************************************************************
 *  函数名称：void Get_Bin_Image (unsigned char mode)
 *  功能说明：图像二值化到Bin_Image[][]
 *  参数说明：mode  ：
 *    0：使用大津法阈值
 *    1：使用平均阈值
 *    2: sobel 算子改进型  手动阈值，同时输出改为提取边沿的图像
 *    3：sobel 算子改进型   动态阈值，同时输出改为提取边沿的图像
 *  函数返回：无
 *  修改时间：2020年10月28日
 *  备    注：  Get_Bin_Image(0); //使用大津法二值化
 *************************************************************************/
void Get_Bin_Image (unsigned char mode)
{
    unsigned short i = 0, j = 0;
    unsigned short Threshold = 0;
    unsigned long tv = 0;
    //char txt[16];

    if (mode == 0)
    {
        Threshold = GetOSTU(Image_Use);  //大津法阈值
    }
    if (mode == 1)
    {
        //累加
        for (i = 0; i < USEH; i++)
        {
            for (j = 0; j < USEW; j++)
            {
                tv += mt9v03x_image[i][j];   //累加
            }
        }
        Threshold =(unsigned short)(tv / USEH / USEW);   //求平均值,光线越暗越小，全黑约35，对着屏幕约160，一般情况下大约100
        Threshold = Threshold + 20;      //此处阈值设置，根据环境的光线来设定
    }
    else if (mode == 2)
    {
        Threshold = 125;                          //手动调节阈值
        lq_sobel(Image_Use, Bin_Image, (unsigned char) Threshold);

        return;

    }
    else if (mode == 3)
    {
        lq_sobelAutoThreshold(Image_Use, Bin_Image);  //动态调节阈值
        return;
    }

    /* 二值化 */
    for (i = 0; i < USEH; i++)
    {
        for (j = 0; j < USEW; j++)
        {
            if (Image_Use[i][j] > Threshold) //数值越大，显示的内容越多，较浅的图像也能显示出来
                Bin_Image[i][j] = 1;
            else
                Bin_Image[i][j] = 0;
        }
    }
}




void Get_Use_Image(void)
{
    short i = 0, j = 0, row = 0, line = 0;

    for (i = 0; i < MT9V03X_H; i += 2)          //神眼高 120 / 2  = 60，
    // for (i = 0; i < IMAGEH; i += 3)       //OV7725高 240 / 3  = 80，
    {
        for (j = 0; j <= MT9V03X_W; j += 2)     //神眼宽188 / 2  = 94，
        // for (j = 0; j <= IMAGEW; j += 3)  //OV7725宽320 / 3  = 106，
        {
            Image_Use[row][line] = mt9v03x_image[i][j];
            line++;
        }
        line = 0;
        row++;
    }
}

void Bin_Image_Filter (void)
{
    sint16 nr; //行
    sint16 nc; //列

    for (nr = 1; nr < USEH - 2; nr++)
    {
        for (nc = 1; nc < USEW - 2; nc = nc + 1)
        {
            if ((Bin_Image[nr][nc] == 0)
                    && (Bin_Image[nr - 1][nc] + Bin_Image[nr + 1][nc] + Bin_Image[nr][nc + 1] + Bin_Image[nr][nc - 1] > 2))
            {
                Bin_Image[nr][nc] = 1;
            }
            else if ((Bin_Image[nr][nc] == 1)
                    && (Bin_Image[nr - 1][nc] + Bin_Image[nr + 1][nc] + Bin_Image[nr][nc + 1] + Bin_Image[nr][nc - 1] < 2))
            {
                Bin_Image[nr][nc] = 0;
            }
        }
    }
}

sint16 OFFSET0 = 0;      //最远处，赛道中心值综合偏移量
sint16 OFFSET1 = 0;      //第二格
sint16 OFFSET2 = 0;      //最近，第三格

/***************************************************************************
 *                                                                          *
 *  函数名称：Seek_Road(void)                                           *
 *  功能说明：寻找白色区域偏差值                                            *
 *  参数说明：无                                                            *
 *  函数返回：值的大小                                                      *
 *  修改时间：2017-07-16                                                    *
 *  备    注：以中间为0，左侧减一，右侧加一，数值代表1的面积                *
 *            计算区域从第一行开始到倒数第二行结束。                        *
 *            如果面积为负数，数值越大说明越偏左边；                        *
 *            如果面积为正数，数值越大说明越偏右边。                        *
 ***************************************************************************/
void Seek_Road (void)
{
    sint16 nr; //行
    sint16 nc; //列
    sint16 temp = 0; //临时数值
    //for(nr=1; nr<MAX_ROW-1; nr++)
    temp = 0;
    for (nr = 8; nr < 24; nr++)
    {
        for (nc = USEW / 2; nc < USEW; nc = nc + 1)
        {
            if (Bin_Image[nr][nc])
            {
                ++temp;
            }
        }
        for (nc = 0; nc < USEW / 2; nc = nc + 1)
        {
            if (Bin_Image[nr][nc])
            {
                --temp;
            }
        }
    }
    OFFSET0 = temp;
    temp = 0;
    for (nr = 24; nr < 40; nr++)
    {
        for (nc = USEW / 2; nc < USEW; nc = nc + 1)
        {
            if (Bin_Image[nr][nc])
            {
                ++temp;
            }
        }
        for (nc = 0; nc < USEW / 2; nc = nc + 1)
        {
            if (Bin_Image[nr][nc])
            {
                --temp;
            }
        }
    }
    OFFSET1 = temp;
    temp = 0;
    for (nr = 40; nr < 56; nr++)
    {
        for (nc = USEW / 2; nc < USEW; nc = nc + 1)
        {
            if (Bin_Image[nr][nc])
            {
                ++temp;
            }
        }
        for (nc = 0; nc < USEW / 2; nc = nc + 1)
        {
            if (Bin_Image[nr][nc])
            {
                --temp;
            }
        }
    }
    OFFSET2 = temp;
    return;
}



int8 brim_L[USEH] = {0}, //左边线(屏上左线)
     brim_R[USEH] = {0}, //右边线
     Deal_flag[USEH];

int8 wide[USEH] = {0},    //赛道宽度
     center[USEH],
            Y_wide;  //中线

uint8 valid_rowL, valid_rowR, crossroad_flag, junction, junction_cnt = 0,
      Y_row, stop_mark;   //边线有效行数
uint8 seek_finish_flag;
/**
 *  @brief  线跟踪法提取边线
 *  @param  None
 *  @return None
 */
void seek_side(){
    int8 row, column, start_col, starting_point = 0;
    uint8 lose_row = 0;
    static int8 last_leftcol = 0, last_rightcol = USEW-1;
    uint8 Yl_cnt ,Yr_cnt;

    valid_rowL = valid_rowR = crossroad_flag = Yl_cnt = Yr_cnt = seek_finish_flag = 0;
    for(row = 5; row < 50; row++){
        //Deal_flag[row] = 0;
/***************************************寻右线***************************************************/
        if(starting_point & 0x1){       //已搜寻到起点, 跟踪巡线
            start_col = last_rightcol + OFFSET; //以上行边界左移OFFSET个像素点为起点
            if(start_col > USEW - 2){
                start_col = USEW - 2;
            }
        }
        else{                           //未达存在边界的行, 从中间开始搜索
            start_col = USEW / 2 ;
        }

        brim_R[row] = -1;
        for(column = start_col; column > 1; column--){
            if(Bin_Image[row][column] && Bin_Image[row][column - 1] && !Bin_Image[row][column + 1]){
                brim_R[row] = USEW-1 - column;
                last_rightcol = column;
                starting_point |= 0x1;  //找到起点的标志

                if(row > 25 && brim_R[row] > brim_R[row - 1] && brim_R[row] - brim_R[row - 1] < 3){
                    ++Yr_cnt;
                }

                break;
            }
        }


/***************************************寻左线***************************************************/
        if(starting_point & 0x2){
            start_col = last_leftcol - OFFSET; //右偏
            if(start_col < 1)
                start_col = 1;
        }
        else
        {
            start_col = USEW / 2 + 1;
        }

        brim_L[row] = 0;
        for(column = start_col; column < USEW-2; column++){
            if(Bin_Image[row][column]&& Bin_Image[row][column + 1] &&  !Bin_Image[row][column - 1]){
                brim_L[row] = USEW-1 - column;
                last_leftcol = column;
                starting_point |= 0x2;

                if(!road_flag && row > 25 && brim_L[row] < brim_L[row - 1] && brim_L[row] - brim_L[row - 1] > -3){
                    ++Yl_cnt;
                }

                break;
            }
        }

/*****************************合成中线*****************************/
        center[row] = -1;

        /*** 十字 ***/
        if(!brim_L[row] && brim_R[row] < 0){
            if(row > 29 && row < 50)
                lose_row ++;

            if(lose_row > 8){
                starting_point = 0;

                if(!crossroad_flag && valid_rowL + valid_rowR > 15)
                    crossroad_flag = row;
            }
            continue;   //两边线均未找到, 跳过无效行
        }//end if

        /***单边及赛道宽度补中线***/
        else if(!brim_L[row]){

            if(wide[row] > 1){
                valid_rowR ++;
                center[row] = brim_R[row] - (wide[row] / 2);

                if(center[row] < 1)
                    center[row] = 1;
            }

        }
        else if(brim_R[row] < 0){
            if(wide[row] > 1){
                valid_rowL ++;
                center[row] = brim_L[row] + (wide[row] / 2);

                if(center[row] > USEW-2)
                    center[row] = USEW-2;
            }
        }

        /***常规合成中线***/
        else {
            if(brim_R[row] - brim_L[row] > 10){      //宽度合理
                valid_rowL ++, valid_rowR ++;

                center[row] = (brim_L[row] + brim_R[row]) / 2;
                if(!run_start){
                    wide[row] = brim_R[row] - brim_L[row];
                }
            }else{
                wide[row] = 0;
                val_row = row;
            }
        }//end_else

        if(Yl_cnt > 2 && Yr_cnt > 2 && finish_sw && !ramp_flag && bias < 12 && bias > -12 && (stopcar & 0x80)){
            junction |= 1;
//            buzzer_ON;
        }/* end if */
    }
    seek_finish_flag = 1;
}

uint8 jump_limit()
{
    uint8 line;

    for(line = START_LINE + 1; line < USEH - 10; line++)
    {
        if(abs_sub(center[line],center[line+1]) > JUMP_LIMIT)
            return line;
        else
            center[line] = (center[line-1] + center[line] + center[line+1]) / 3;
    }

    return line;
}


uint8 brim_L_is_straight(uint8 start_row, uint8 end_row, uint8 cnt)
{
    uint8 cnt_t = 0;

    for(uint8 row_t = start_row; row_t < end_row; row_t++)
    {
        if(brim_L[row_t] >= brim_L[row_t - 1] && brim_L[row_t] - brim_L[row_t - 1] < 3){
            ++cnt_t;
        }
        else if(cnt_t > 9)
        {
            break;
        }
    }
    if(cnt_t > cnt){
        return 1;
    }

    return 0;
}

uint8 brim_R_is_straight(uint8 start_row, uint8 end_row, uint8 cnt)
{
    uint8 cnt_t = 0;

    for(uint8 row_t = start_row; row_t < end_row; row_t++)
    {
        if(brim_R[row_t] <= brim_R[row_t - 1] && brim_R[row_t] - brim_R[row_t - 1] > -3){
            cnt_t++;
        }
        else if(cnt_t > 9)
        {
            break;
        }
    }
    if(cnt_t > cnt){
        return 1;
    }

    return 0;
}

void useless_remove()
{

}
